#include "gameMath.h"
#include "vector.h"
#include "constants.h"
#include <math.h>
#include <float.h>

//returns the absolute value of a given number
float absoluteValue(const float value)
{
	if (value < 0)
		return -value;

	return value;
}

//returns the cross product of two given vectors
//(a perpendicular vector from two given vectors)
Vector crossProduct(Vector vector1, Vector vector2)
{
	Vector normalVector;
	normalVector[0] = ((vector1[1] * vector2[2]) - (vector1[2] * vector2[1]));
	normalVector[1] = ((vector1[2] * vector2[0]) - (vector1[0] * vector2[2]));
	normalVector[2] = ((vector1[0] * vector2[1]) - (vector1[1] * vector2[0]));

	return normalVector;
}

//returns the dot product from two given vectors
float dotProduct(Vector vector1, Vector vector2)
{
	return ( (vector1[0] * vector2[0]) + (vector1[1] * vector2[1]) + (vector1[2] * vector2[2]) );
}

//returns the angle between two given errors
double angleBetweenVectors(Vector vector1, Vector vector2)
{
	double dotProductResult = dotProduct(vector1, vector2);				
	double vectorsMagnitude = vectorMagnitude(vector1) * vectorMagnitude(vector2) ;
	double angle = acos(dotProductResult / vectorsMagnitude);

	if(_isnan(angle))
		return 0;

	return(angle);
}

//returns the vector between two given points
Vector vectorFromTwoPoints(Vector point1, Vector point2)
{
	Vector vector;

	vector[0] = point1[0] - point2[0];
	vector[1] = point1[1] - point2[1];
	vector[2] = point1[2] - point2[2];

	return vector;
}

//checks whether a given point is inside a triangle
bool insideTriangle(Vector point, Vector triangleVertices[])
{
	const double MATCH_FACTOR = 0.99;		// Used to cover up the error in floating point
	double angle = 0.0;
	Vector vectorA, vectorB;
	
	//create vectors from all vertices to the given point
	//calculate angles between them and add
	for (int i = 0; i < 3; i++)
	{	
		vectorA = triangleVertices[i] - point;
		vectorB = triangleVertices[(i + 1) % 3] - point;
												
		angle += angleBetweenVectors(vectorA, vectorB);
	}

	//if the summed angle equals to 360 it means
	//that the point is inside the triangle										
	if(angle >= (MATCH_FACTOR * (2.0 * Constants::PI)))
		return true;
		
	return false;
}

//returns the magnitude of a given vector
float vectorMagnitude(Vector vector)
{
	return (float)sqrt( (vector[0] * vector[0]) + 
						(vector[1] * vector[1]) + 
						(vector[2] * vector[2]) );
}

//return the normalized vector of given vector
Vector normalize(Vector normalVector)
{
	float magnitude = vectorMagnitude(normalVector);

	normalVector[0] /= magnitude;
	normalVector[1] /= magnitude;
	normalVector[2] /= magnitude;

	return normalVector;
}

//returns the normal vector of a given triangle
Vector triangleNormal(Vector triangle[])
{
	Vector vector1 = vectorFromTwoPoints(triangle[2], triangle[0]);
	Vector vector2 = vectorFromTwoPoints(triangle[1], triangle[0]);

	Vector normalVector = crossProduct(vector1, vector2);

	normalVector = normalize(normalVector);	

	return normalVector;
}

//returns the distance from origin to a given plane
float planeOriginDistance(Vector normalVector, Vector point)
{	
	float distance = 0;

	distance = - ((normalVector[0] * point[0]) +
				 (normalVector[1] * point[1]) +
				 (normalVector[2] * point[2]));

	return distance;
}

//return the distance between the 2 points
float distanceBetweenPoints(Vector point1, Vector point2)
{
	double distance = sqrt( (point2[0] - point1[0]) * (point2[0] - point1[0]) +
						    (point2[1] - point1[1]) * (point2[1] - point1[1]) +
						    (point2[2] - point1[2]) * (point2[2] - point1[2]) );

	return (float)distance;
}